#include <iostream>
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Float64MultiArray.h"
#include "std_msgs/Float32MultiArray.h"
#include "small_car.h"
#include "std_msgs/Int16MultiArray.h"
#include "navigation_msg/odom.h"
#include "std_msgs/Int8.h"//ROS自定义的数据类型String所在的头文件
#include "didi_msgs/Pose2DStamped.h"
#include <vector>
#include <queue>
#include <cmath>

#define  TURN_LEFT 1
#define  TURN_RIGHT 2
#define  TURN_STOP  0
#define  ANGLE_TARGET_SMALL 0.025
#define  ANGLE_P_SMALL  15
#define  ANGLE_I_SMALL  1
#define  ANGLE_D_SMALL  2
#define  DEF_SLAM_RESULT_SIZE  8

using namespace std;

namespace move_base {

Small_car::Small_car():
  move_state(0),
  contact_flag(0),
  move_speed(0),
  control_flag(0), 
  right_wheel_speed(0),
  left_wheel_speed(0),
  motor_wheel_speed(0),
  disc_turn_angle(0),
  disc_turn_pulse(0),
  turn_enable(0),
  imu_x(0.0),
  imu_y(0.0),
  imu_angle(0.0),
  temp_slam_x(0.0),
  temp_slam_y(0.0),
  temp_slam_z(0.0),
  target_slam_x(0.0),
  target_slam_y(0.0),
  target_slam_z(0.0),
  slam_angle(0.0),
  gps_x(0.0),
  gps_y(0.0),
  gps_angle(0.0),
  slam_given_angle(0.0),
  turn_right_enable(1),
  turn_left_enable(1),
  obstack_inqure(0),
  obstack_appear(false)

{
   cout<<"buld small_car success"<<endl;
   //q1.push(0);
   Control_Command= motion_handle.advertise<std_msgs::Int16MultiArray>("key_board/topic_command",10);
   pub_slam_data  = motion_handle.advertise<didi_msgs::Pose2DStamped>("test_traj_display", 10);
   motion_handle.param("turn_enable",turn_enable,0);
   motion_handle.param("turn_left_enable",turn_left_enable,0);
   motion_handle.param("turn_right_enable",turn_right_enable,0);
}      
     
Small_car::~Small_car(){
 
  cout<<"destory small_car success"<<endl;
 
}

void Small_car::set_car_state(uint16_t movestate){

         move_state=movestate;

}

uint16_t Small_car::get_car_state(){
         
         return move_state;

 }

 void Small_car::set_car_contactflag(uint16_t contactflag){
         
        contact_flag=contactflag;
 }

 uint16_t Small_car::get_car_contactflag(){

         return contact_flag;
 }

 void Small_car::set_car_speed(uint16_t speed){
        
       	 move_speed=speed;
  }

 uint16_t Small_car::get_car_speed(){

         return move_speed;

 }


//stop the car
void Small_car::stop_car()
{
   
}

//make car turn right
void Small_car::turn_right()
{
      
     
}

////make car turn left
void Small_car::turn_left(){
	
	
}

////make car turn stop
void Small_car::turn_stop(){
	
	

}

//make car go forward
void Small_car::go_forward()
{
     
        
}

//make car go backward
void Small_car::go_backward(){

     
}


//----------subscribe std_msgs/Float64MultiArray messages----------
void Small_car::doubleCallback(const std_msgs::Float64MultiArrayConstPtr& vec_double)
{

}


void  Small_car::send_control_command(int16_t speed_enable,int16_t target_speed,int16_t dir_enable,int16_t target_angle,int16_t  target_dir){


     std_msgs::Int16MultiArray con_com;
     con_com.data.push_back(speed_enable);
     con_com.data.push_back(target_speed);
     con_com.data.push_back(dir_enable);
     con_com.data.push_back(target_angle);
     con_com.data.push_back(target_dir);
     Control_Command.publish(con_com);

}

/*According to the deviation of angle, the control of direction angle is obtained by PID calculation.*/
void Small_car::follow_given_target(float given_angle){



    float  car_p=0,car_i=0,car_d=0;
    static float angle_error[3]={0};
    int16_t set_speed=0;
    int16_t set_angle=0;
    float sum_angle_err=0;

    angle_error[2]=angle_error[1];
    angle_error[1]=angle_error[0];
    angle_error[0] =given_angle;

    car_p=ANGLE_P_SMALL*(angle_error[0]-angle_error[1])/5;
    car_d=ANGLE_D_SMALL*(angle_error[0]-2*angle_error[1]+angle_error[2])/5;
    car_i=ANGLE_I_SMALL*angle_error[0]/5;

    sum_angle_err=car_p+car_i+car_d;

    if(fabs(sum_angle_err)<=ANGLE_TARGET_SMALL)    move_state=TURN_STOP;
    else if(sum_angle_err>ANGLE_TARGET_SMALL)      move_state=TURN_LEFT;
    else if(sum_angle_err<-ANGLE_TARGET_SMALL)     move_state=TURN_RIGHT;

   // printf("sum_angle_err:%f \r\n",sum_angle_err);

    if(fabs(sum_angle_err)>0.1) {set_speed=1150;set_angle=400;}
    else if(fabs(sum_angle_err)>0.08) {set_speed=1140;set_angle=300;}
    else if(fabs(sum_angle_err)>0.065) {set_speed=1138;set_angle=250;}
    else if(fabs(sum_angle_err)>0.055) {set_speed=1125;set_angle=200;}
    else if(fabs(sum_angle_err)>0.035) {set_speed=1120;set_angle=180;}
    else if(fabs(sum_angle_err)>0) {set_speed=1120;set_angle=100;}

    switch(move_state){

            case  TURN_LEFT:

                  if( turn_left_enable==1) {

                      send_control_command(1,set_speed,1,set_angle,0);
                      usleep(10000);

                  }
                   cout<<"the car is turning left **************"<<endl;
                   break;

            case  TURN_RIGHT:

                   if( turn_right_enable==1) {

                       send_control_command(1,set_speed,1,set_angle,1);
                       usleep(10000);

                    }
                   cout<<"the car is turning right **************"<<endl;
                   break;

            case  TURN_STOP:

                   send_control_command(0,2000,0,0,0);
                   cout<<"the car stop  turning **************"<<endl;
                   break;

             default:
                   break;
    }
}





//----------subscribe std_msgs/Float32MultiArray messages----------
void Small_car::SlamORBCallback(const std_msgs::Float32MultiArrayConstPtr& vec_double)
{


        float slam_result[DEF_SLAM_RESULT_SIZE];
        for(int i=0;i<DEF_SLAM_RESULT_SIZE;i++) slam_result[i]=vec_double->data[i];
        printf("double_array: ");
        /*for(int i=0;i<DEF_SLAM_RESULT_SIZE;i++) printf("%f ",vec_double->data[i]);*/
        printf("\n");

        slam_given_angle=slam_result[0];
        temp_slam_x=vec_double->data[2];
        temp_slam_z=vec_double->data[4];
        target_slam_x=vec_double->data[5];
        target_slam_z=vec_double->data[7];

        slam_distance=sqrt(pow(temp_slam_x,2)+pow(temp_slam_z,2));

        printf("%f,%f,%f,%f,%f,%f\r\n",slam_given_angle,temp_slam_x,
               temp_slam_z,target_slam_x,target_slam_z,slam_distance);

}


void Small_car::slam_map_display(){

    didi_msgs::Pose2DStamped p;
    p.header.frame_id = "/map";
    p.header.stamp = ros::Time::now();

    p.trajpoint.x =temp_slam_x;
    p.trajpoint.y = temp_slam_z;
    p.trajpoint.theta = slam_given_angle;

    pub_slam_data.publish( p );
}

//----------subscribe std_msgs/Int8 messages----------
void Small_car::ladarCallback(const std_msgs::Int8 ladar_diatance)
{

      //obstack_dis.push(ladar_diatance.data);

      if(obstack_dis.size()>=10){

         obstack_inqure=obstack_inqure-obstack_dis.front();
         obstack_dis.pop();
         obstack_dis.push(ladar_diatance.data);
         obstack_inqure=obstack_inqure+ladar_diatance.data;

      }else{

        obstack_inqure=obstack_inqure+ladar_diatance.data;
        obstack_dis.push(ladar_diatance.data);
      }

      if(obstack_inqure>=5){

          send_control_command(1,2000,0,0,0);
          obstack_appear=true;
          printf("the obstack is in front ,please stop the car\r\n");
          usleep(10000);


      }else if(obstack_appear==true){

          obstack_appear=false;
          send_control_command(1,1012,0,0,0);
          printf("the obstack has removed,please start the car\r\n");
          usleep(10000);

      }

}

void Small_car::ImuDataCallback(const std_msgs::Float64MultiArrayConstPtr& imu_message){

     imu_angle=imu_message->data[0];
     //printf("imu_angle:%f\r\n",imu_angle);

 }


void Small_car::run(){


      ros::Subscriber SLAM_ORB = motion_handle.subscribe("ORB_SLAM/angle_dist", 5, &Small_car::SlamORBCallback,this);

      ros::Subscriber sub_ladar = motion_handle.subscribe("serial/ladar_distance_data",
          10, &Small_car::ladarCallback,this);

      ros::Subscriber imu_sensor=motion_handle.subscribe("sensor/imu",
          5,&Small_car::ImuDataCallback,this);

      ros::Rate loop_rate(100);//一个ros::Rate对象允许user制定循环的频率

      while(motion_handle.ok()){

         motion_handle.getParam("turn_enable",turn_enable);
         motion_handle.getParam("turn_left_enable",turn_left_enable);
         motion_handle.getParam("turn_left_enable",turn_right_enable);

         if((turn_enable==1)&&(obstack_appear==false)){

             follow_given_target(slam_given_angle);
         }

         ros::spinOnce();
      }
}

}

